// 2022-4-25 0.0.0.1A

#include <ros/ros.h>
#include <angles/angles.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <linux/input.h>
#include <geometry_msgs/Twist.h>
#include <tecbot_msgs/cmd_vel_mode_request.h>
#include <common/common_state_code.h>
#include <navigation/navigation_state_code.h>

#define NODE_NAME "simulation_handler"
#define TAG "机器人仿真速度发布器"

using namespace std;

// 定义速度控制模式
bool cmd_vel_manual_mode = true;

// 定义仿真速度发布器
ros::Publisher cmd_vel_sim_pub;

bool cmd_vel_mode_callback(tecbot_msgs::cmd_vel_mode_request::Request &req, tecbot_msgs::cmd_vel_mode_request::Response &resp)
{
    cmd_vel_manual_mode = req.manual;

    resp.result = CMD_RESPONSE_OK;

    return true;
}

void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (!cmd_vel_manual_mode)
    {
        cmd_vel_sim_pub.publish(*msg);
    }
}

void cmd_vel_manual_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (cmd_vel_manual_mode)
    {
        cmd_vel_sim_pub.publish(*msg);
    }
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, NODE_NAME);

    // 设置控制台本地化，以显示中文
    setlocale(LC_CTYPE, "zh_CN.utf8");

    // 打印调试信息
    ROS_INFO("%s：节点已启动。", TAG);

    // 定义ROS节点处理程序
    ros::NodeHandle node_handle;

    // 定义ROS参数获取程序
    ros::NodeHandle node_handle_param("~/simulation");

    // 定义速度发布模式
    ros::ServiceServer cmd_vel_mode_server = node_handle.advertiseService("tecbot/control/cmd_vel_mode/request", cmd_vel_mode_callback);

    // 定义速度订阅器
    ros::Subscriber cmd_vel_sub = node_handle.subscribe<geometry_msgs::Twist>("tecbot/control/cmd_vel", 10, cmd_vel_callback);
    ros::Subscriber cmd_vel_manual_sub = node_handle.subscribe<geometry_msgs::Twist>("tecbot/control/cmd_vel_manual", 10, cmd_vel_manual_callback);

    // 定义速度发布器
    cmd_vel_sim_pub = node_handle.advertise<geometry_msgs::Twist>("tecbot/control/cmd_vel_sim", 1);

    ros::spin();

    return 0;
}
